#include <vector>
#include <string> 
#include <algorithm>
#include <iostream>
#include <cmath>
#include <stdio.h>
using namespace std;
typedef double Real;
double euclideanDistance(const vector<Real> &actual, const vector<Real> &training);
Real cosineDistance(std::vector<Real> &A, std::vector<Real> &B)
{
    Real mul = 0.0;
    Real d_a = 0.0;
    Real d_b = 0.0 ;

    if (A.size() != B.size())
    {
        cout<<"Vector A and Vector B are not the same size"<<endl;
        return 1;
    }

    // Prevent Division by zero
    if (A.size() < 1)
    {
        cout<<"Vector A and Vector B are empty"<<endl;
        return 1;
    }

    std::vector<Real>::iterator B_iter = B.begin();
    std::vector<Real>::iterator A_iter = A.begin();
    for( ; A_iter != A.end(); A_iter++ , B_iter++ )
    {
        mul += *A_iter * *B_iter;
        d_a += *A_iter * *A_iter;
        d_b += *B_iter * *B_iter;
    }

    if (d_a == 0.0f || d_b == 0.0f)
    {
        cout<<
                "cosine similarity is not defined whenever one or both "
                "input vectors are zero-vectors."<<endl;
                return 1;
    }

    return 1-mul / (sqrt(d_a) * sqrt(d_b));
}

double DTWDistance( std::vector< std::vector<Real> > &actual,  std::vector< std::vector<Real> > &training)
{
    int m = actual.size();
    int n = training.size();
    vector< vector<double> > cost (m, vector<double>(n,0));
    cost[0][0] = euclideanDistance(actual[0], training[0]);

    for(int i = 1; i < m; i++)
            cost[i][0] = cost[i-1][0] + euclideanDistance(actual[i], training[0]);
    for(int j = 1; j < n; j++)
            cost[0][j] = cost[0][j-1] + euclideanDistance(actual[0], training[j]);

    for(int i = 1; i < m; i++)
            for(int j = 1; j < n; j++)
                cost[i][j] = std::min(cost[i-1][j], std::min(cost[i][j-1], cost[i-1][j-1])) 
                    + euclideanDistance(actual[i],training[j]);
    
    return cost[m-1][n-1];
}

struct  DtwPoint
{
    /**
     * Creates the point with default values.
     */
    DtwPoint():
        x(0), y(0), dLocal(0.0), dAccumulated(0.0), previous(0)
    {
    }

    /**
     * Creates the point and associates it with given coordinates.
     *
     * @param x_ x coordinate in DTW array
     * @param y_ y coordinate in DTW array
     * @param distanceLocal value of local distance at (x, y)
     */
    DtwPoint(std::size_t x_, std::size_t y_, double distanceLocal = 0.0):
        x(x_), y(y_), dLocal(distanceLocal),
        // at the edges set accumulated distance to local. otherwise 0
        dAccumulated((0 == x || 0 == y) ? dLocal : 0.0),
        previous(0)
    {
    }

    /**
     * X coordinate of the point in the DTW array.
     */
    std::size_t x;

    /**
     * Y coordinate of the point in the DTW array.
     */
    std::size_t y;

    /**
     * Local distance at this point.
     */
    double dLocal;

    /**
     * Accumulated distance at this point.
     */
    double dAccumulated;

    /**
     * Non-owning pointer to previous point in the lowest-cost path.
     */
    DtwPoint* previous;
};

    /**
     * Type of compared data - vectors of features, which themselves are
     * vectors of doubles.
     */
    typedef std::vector< std::vector<Real> > DtwDataType;
    typedef vector< vector< vector<Real> > > DtwDataType2;
    /**
     * Type of DTW point array.
     */
    typedef std::vector< std::vector<DtwPoint> > DtwPointsArrayType;

    /**
     * Lowest-cost path is a vector of points.
     */
    typedef std::vector<DtwPoint> DtwPathType;

    /**
     * Dynamic Time Warping implementation.
     */
    class Dtw
    {
    public:
        /**
         * Type of lowest-cost passes between points.
         */
        enum PassType {Neighbors, Diagonals};

        /**
         * Creates the DTW algorithm wrapper object.
         *
         * @param distanceFunction which function to use for calculating distance
         * @param passType pass type - how to move through distance array
         */
        Dtw(
            PassType passType = Neighbors):
            m_passType(passType),
            m_points()
        {
        }

        double getDistance( DtwDataType& from,  DtwDataType& to);
        //double getDistance2( DtwDataType2& from,  DtwDataType2& to);
        /**
         * Returns a const reference to the point array.
         *
         * @return DTW points
         */
        const DtwPointsArrayType& getPoints() 
        {
            return m_points;
        }

        /**
         * Returns the final point on the DTW path (in the top right corner).
         *
         * @return a DTW point
         */
        DtwPoint getFinalPoint() 
        {
            return m_points[m_fromSize - 1][m_toSize - 1];
        }

        DtwPathType getPath() ;

    private:
        /**
         * Type of passes between points.
         */
        PassType m_passType;

        /**
         * Array of DTW points.
         */
        DtwPointsArrayType m_points;

        /**
         * Coordinates of the top right corner of the points array.
         */
        std::size_t m_fromSize, m_toSize;
    };

    void drawDtwPath( Dtw& dtw, std::size_t xSize, std::size_t ySize);


    // the implentation of the functions 

    double euclideanDistance(const vector<Real> &actual, const vector<Real> &training)
{

    double distance = 0.0;

    for (unsigned i = 0; i < actual.size(); i++)
    {
        distance = pow((actual[i] - training[i]), 2);
    }

    return sqrt(distance);

}
void drawDtwPath( Dtw& dtw, std::size_t xSize, std::size_t ySize)
{
    std::vector< std::vector<unsigned char> > data(ySize, vector<unsigned char>(xSize, '.'));

    DtwPathType path = dtw.getPath();

    cout<<"path size: "<< path.size() <<endl;
    getchar();
    for (unsigned int i=0; i<path.size(); i++)
    {      
        data[path[i].x][path[i].y] = 'o';
        cout<< "x: "<< path[i].x << " y: "<< path[i].y << " local dis: "<< path[i].dLocal<<endl;
    }
    for(unsigned int i=0; i <data.size(); i++)
    {
        for(unsigned int j=0; j<data[i].size(); j++)
            cout<<data[i][j];
        cout<<endl;
    }
       
}
/**
 * Computes the distance between two sets of data.
 *
 * @param from first vector of features
 * @param to second vector of features
 * @return double DTW distance
 */
double Dtw::getDistance( DtwDataType& from,  DtwDataType& to)
{
    m_fromSize = from.size();
    m_toSize = to.size();

    // fill the local distances array
    m_points.clear();
    m_points.resize(m_fromSize);
    for (std::size_t i = 0; i < m_fromSize; ++i)
    {
        m_points[i].reserve(m_toSize);
        for (std::size_t j = 0; j < m_toSize; ++j)
        {
            // use emplace_back, once all compilers support it correctly
                m_points[i].push_back(
                DtwPoint(i, j, cosineDistance(from[i], to[j])
            ));
        }
    }
      // the actual pathfinding algorithm
    DtwPoint *top = NULL, *center = NULL, *bottom = NULL, *previous = NULL;
    /*
    m_points[0][0].dAccumulated=m_points[0][0].dLocal;
    for(std::size_t i=1; i<m_toSize; i++)
    {
        m_points[0][i].dAccumulated = m_points[0][i].dLocal + m_points[0][i-1].dLocal;
        m_points[0][i].previous = &m_points[0][i-1];
    }
    for (std::size_t i = 1; i < m_fromSize; ++i)
    {
        m_points[i][0].dAccumulated = m_points[i][0].dLocal + m_points[i-1][0].dLocal;
        m_points[i][0].previous = &m_points[i-1][0];
        for (std::size_t j = 1; j < m_toSize; ++j)
        {
            center = &m_points[i - 1][j - 1];
            if (Neighbors == m_passType)
            {
                top = &m_points[i - 1][j];
                bottom = &m_points[i][j - 1];
                //bottom = &m_points[i-1][j-1]; //updated by lichuan
            }
            else // Diagonals
            {
                if (i > 1 && j > 1)
                {
                    top = &m_points[i - 2][j - 1];
                    bottom = &m_points[i - 1][j - 2];
                }
                else
                {
                    top = &m_points[i - 1][j];
                    bottom = &m_points[i][j - 1];
                }
            }

            if (top->dAccumulated < center->dAccumulated)
            {
                previous = top;
            }
            else
            {
                previous = center;
            }

            if (bottom->dAccumulated  < previous->dAccumulated)
            {
                previous = bottom;
            }

            m_points[i][j].dAccumulated = m_points[i][j].dLocal + previous->dAccumulated;
            m_points[i][j].previous = previous;
        }
    }*/
	    m_points[0][0].dAccumulated=m_points[0][0].dLocal;
		for(std::size_t i=1; i<m_toSize; i++)
		{
			m_points[0][i].dAccumulated = m_points[0][i].dLocal + m_points[0][i-1].dLocal;
			m_points[0][i].previous = &m_points[0][i-1];
		}
	
        for (std::size_t i = 1; i < m_fromSize; ++i)
        {
			m_points[i][0].dAccumulated = m_points[i][0].dLocal + m_points[i-1][0].dLocal;
			m_points[i][0].previous = &m_points[i-1][0];
            for (std::size_t j = 1; j < m_toSize; ++j)
            {
                center = &m_points[i - 1][j - 1];
                if (Neighbors == m_passType)
                {
                    top = &m_points[i - 1][j];
                    bottom = &m_points[i][j - 1];
                }
                else // Diagonals
                {
                    if (i > 1 && j > 1)
                    {
                        top = &m_points[i - 2][j - 1];
                        bottom = &m_points[i - 1][j - 2];
                    }
                    else
                    {
                        top = &m_points[i - 1][j];
                        bottom = &m_points[i][j - 1];
                    }
                }

                if (top->dAccumulated < center->dAccumulated)
                {
                    previous = top;
                }
                else
                {
                    previous = center;
                }

                if (bottom->dAccumulated < previous->dAccumulated)
                {
                    previous = bottom;
                }

                m_points[i][j].dAccumulated = m_points[i][j].dLocal + previous->dAccumulated;
                m_points[i][j].previous = previous;
            }
        }
    return getFinalPoint().dAccumulated;
}


/**
 * Computes the distance between two sets of data.
 *
 * @param from first vector of features
 * @param to second vector of features
 * @return double DTW distance
 */
 /*
double Dtw::getDistance2( DtwDataType2& from,  DtwDataType2& to)
{
    m_fromSize = from.size();
    m_toSize = to.size();

    // fill the local distances array
    m_points.clear();
    m_points.resize(m_fromSize);
    for (std::size_t i = 0; i < m_fromSize; ++i)
    {
        m_points[i].reserve(m_toSize);
        for (std::size_t j = 0; j < m_toSize; ++j)
        {
            // use emplace_back, once all compilers support it correctly
            m_points[i].push_back(
                DtwPoint(i, j, DTWDistance(from[i], to[j])
            ));
        }
    }

    // the actual pathfinding algorithm
    DtwPoint *top = NULL, *center = NULL, *bottom = NULL, *previous = NULL;
    for (std::size_t i = 1; i < m_fromSize; ++i)
    {
        for (std::size_t j = 1; j < m_toSize; ++j)
        {
            center = &m_points[i - 1][j - 1];
            if (Neighbors == m_passType)
            {
                top = &m_points[i - 1][j];
                bottom = &m_points[i][j - 1];
            }
            else // Diagonals
            {
                if (i > 1 && j > 1)
                {
                    top = &m_points[i - 2][j - 1];
                    bottom = &m_points[i - 1][j - 2];
                }
                else
                {
                    top = &m_points[i - 1][j];
                    bottom = &m_points[i][j - 1];
                }
            }

            if (top->dAccumulated < center->dAccumulated)
            {
                previous = top;
            }
            else
            {
                previous = center;
            }

            if (bottom->dAccumulated < previous->dAccumulated)
            {
                previous = bottom;
            }

            m_points[i][j].dAccumulated = m_points[i][j].dLocal + previous->dAccumulated;
            m_points[i][j].previous = previous;
        }
    }

    return getFinalPoint().dAccumulated;
}
*/
/**
 * Returns the lowest-cost path in the DTW array.
 *
 * @return path
 */
DtwPathType Dtw::getPath() 
{
    DtwPathType path;
    DtwPoint finalPoint = getFinalPoint();
    DtwPoint* point = &finalPoint;

    path.push_back(*point);

    while(point->previous)
    {

        point = point->previous;
        path.push_back(*point);

    }

    return path;
}
